/*
 * CDebugDraw.cpp
 *
 *  Created on: Sep 7, 2011
 *      Author: welcome
 */

#include "CDebugDraw.h"
#include <KD/kd.h>

namespace GGEngine {

CDebugDraw::CDebugDraw() {

}

CDebugDraw::~CDebugDraw() {

}
void CDebugDraw::DrawPolygon(const b2Vec2* vertices, int32 vertexCount,
		const b2Color& color) {

	GLfloat* v = (GLfloat*) kdMalloc(vertexCount * 2 * sizeof(GLfloat));

	kdMemset(v, 0, sizeof(v));

	glEnableClientState(GL_VERTEX_ARRAY);

	glColor4f(color.r, color.g, color.b, 1.0f);

	for (int32 i = 0, j = 0; i < vertexCount; ++i, j += 2) {

		v[j] = vertices[i].x;

		v[j + 1] = vertices[i].y;

	}

	glVertexPointer(2, GL_FLOAT, 0, v);

	glDrawArrays(GL_TRIANGLE_FAN, 0, vertexCount);

	glDisableClientState(GL_VERTEX_ARRAY);

	free(v);
}
void CDebugDraw::DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount,
		const b2Color& color) {

	GLfloat* v = (GLfloat*) kdMalloc(vertexCount * 2 * sizeof(GLfloat));

	kdMemset(v, 0, sizeof(v));

	glEnableClientState(GL_VERTEX_ARRAY);

	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glColor4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f);
	for (int32 i = 0, j = 0; i < vertexCount; ++i, j += 2) {
		v[j] = vertices[i].x;
		v[j + 1] = vertices[i].y;
	}
	glVertexPointer(2, GL_FLOAT, 0, v);

	glDrawArrays(GL_TRIANGLE_FAN, 0, vertexCount);

	glDisable(GL_BLEND);

	glColor4f(color.r, color.g, color.b, 1.0f);

	glVertexPointer(2, GL_FLOAT, 0, v);

	glDrawArrays(GL_LINE_LOOP, 0, vertexCount);

	glDisableClientState(GL_VERTEX_ARRAY);

	free(v);
}

#define k_segments  16

void CDebugDraw::DrawCircle(const b2Vec2& center, float32 radius,
		const b2Color& color) {

	const float32 k_increment = 2.0f * b2_pi / k_segments;

	float32 theta = 0.0f;

	GLfloat vertices[k_segments * 2];

	glEnableClientState(GL_VERTEX_ARRAY);

	for (int32 i = 0, j = 0; i < k_segments; ++i, j += 2) {
		b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
		vertices[j] = v.x;
		vertices[j + 1] = v.y;
		theta += k_increment;
	}

	glVertexPointer(2, GL_FLOAT, 0, vertices);

	glColor4f(color.r, color.g, color.b, 1.0f);

	glDrawArrays(GL_LINE_LOOP, 0, k_segments);

	glDisableClientState(GL_VERTEX_ARRAY);
}

void CDebugDraw::DrawSolidCircle(const b2Vec2& center, float32 radius,
		const b2Vec2& axis, const b2Color& color) {

	const float32 k_increment = 2.0f * b2_pi / k_segments;

	float32 theta = 0.0f;

	GLfloat vertices[k_segments * 2] = { 0.0 };

	glEnableClientState(GL_VERTEX_ARRAY);

	glEnable(GL_BLEND);

	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	glColor4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f);

	for (int32 i = 0, j = 0; i < k_segments; ++i, j += 2) {
		b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
		vertices[j] = v.x;
		vertices[j + 1] = v.y;
		theta += k_increment;
	}

	glVertexPointer(2, GL_FLOAT, 0, vertices);

	glDrawArrays(GL_TRIANGLE_FAN, 0, k_segments);

	glDisable(GL_BLEND);
	/*
	 *
	 */
	theta = 0.0f;

	glColor4f(color.r, color.g, color.b, 1.0f);

	for (int32 i = 0, j = 0; i < k_segments; ++i, j += 2) {
		b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
		vertices[j] = v.x;
		vertices[j + 1] = v.y;
		theta += k_increment;
	}

	glVertexPointer(2, GL_FLOAT, 0, vertices);

	glDrawArrays(GL_LINE_LOOP, 0, k_segments);

	/*
	 *
	 */
	b2Vec2 p = center + radius * axis;
	GLfloat vLine[4] = { center.x, center.y, p.x, p.y };

	GLubyte indices[] = { 0, 1 };

	glVertexPointer(2, GL_FLOAT, 0, vLine);

	glDrawElements(GL_LINES, 2, GL_UNSIGNED_BYTE, indices);

	glDisableClientState(GL_VERTEX_ARRAY);
}

void CDebugDraw::DrawPoint(const b2Vec2& p, float32 size, const b2Color& color) {

	GLfloat vertices[2] = { p.x, p.y };

	GLubyte indices[1] = { 0 };

	glEnableClientState(GL_VERTEX_ARRAY);

	glPointSize(size);

	glVertexPointer(2, GL_FLOAT, 0, vertices);

	glColor4f(color.r, color.g, color.b, 1.0f);

	glDrawElements(GL_POINTS, 1, GL_UNSIGNED_BYTE, indices);

	glPointSize(1.0f);

	glDisableClientState(GL_VERTEX_ARRAY);
}
void CDebugDraw::DrawAABB(b2AABB* aabb, const b2Color& c) {

	GLfloat vertices[] = { aabb->lowerBound.x, aabb->lowerBound.y, 0,
			aabb->upperBound.x, aabb->lowerBound.y, 0, aabb->upperBound.x,
			aabb->upperBound.y, 0, aabb->lowerBound.x, aabb->upperBound.y, 0 };

	GLubyte indices[] = { 0, 1, 2, 0, 2, 3 };

	glEnableClientState(GL_VERTEX_ARRAY);

	glVertexPointer(3, GL_FLOAT, 0, vertices);

	glColor4f(c.r, c.g, c.b, 1.0);

	glDrawElements(GL_LINE_LOOP, 6, GL_UNSIGNED_BYTE, indices);

	glDisableClientState(GL_VERTEX_ARRAY);
}

void CDebugDraw::DrawSegment(const b2Vec2& p1, const b2Vec2& p2,
		const b2Color& color) {

	GLfloat vLine[4] = { p1.x, p1.y, p2.x, p2.y };
	GLubyte indices[] = { 0, 1 };

	glEnableClientState(GL_VERTEX_ARRAY);

	glColor4f(color.r, color.g, color.b, 1.0f);

	glVertexPointer(2, GL_FLOAT, 0, vLine);
	glDrawElements(GL_LINES, 2, GL_UNSIGNED_BYTE, indices);

	glDisableClientState(GL_VERTEX_ARRAY);
}
void CDebugDraw::DrawTransform(const b2Transform& xf) {

	b2Vec2 p1 = xf.p, p2, p3;

	const float32 k_axisScale = 0.4f;

	p2 = p1 + k_axisScale * xf.q.GetXAxis();
	p3 = p1 + k_axisScale * xf.q.GetYAxis();

	GLfloat vLine[6] = { p1.x, p1.y, p2.x, p2.y, p3.x, p3.y };

	GLubyte indices1[] = { 0, 1 };

	GLubyte indices2[] = { 0, 2 };

	glEnableClientState(GL_VERTEX_ARRAY);

	glColor4f(1.0f, 0.0f, 0.0f, 1.0f);

	glVertexPointer(2, GL_FLOAT, 0, vLine);
	glDrawElements(GL_LINES, 2, GL_UNSIGNED_BYTE, indices1);

	glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
	glDrawElements(GL_LINES, 2, GL_UNSIGNED_BYTE, indices2);

	glDisableClientState(GL_VERTEX_ARRAY);
}
/*
 * Bullet physics
 */
void CDebugDraw::drawContactPoint(const btVector3& PointOnB,
		const btVector3& normalOnB, btScalar distance, int lifeTime,
		const btVector3& color) {

}

void CDebugDraw::reportErrorWarning(const char* warningString) {
	kdLogMessage(warningString);
}

void CDebugDraw::draw3dText(const btVector3& location, const char* textString) {
	/*
	 * Unimplemented
	 */
}

void CDebugDraw::setDebugMode(int debugMode) {
	this->debugMode = debugMode;
}

int CDebugDraw::getDebugMode() const {
	return this->debugMode;
}

void CDebugDraw::drawLine(const btVector3& from, const btVector3& to,
		const btVector3& color) {
	GLfloat vLine[6] = { from.getX(), from.getY(), from.getZ(),
			to.getX(), to.getY(), to.getZ() };

	GLubyte indices[] = { 0, 1 };

	glEnableClientState(GL_VERTEX_ARRAY);

	glColor4f(color.getX(), color.getY(), color.getZ(), 1.0f);

	glVertexPointer(3, GL_FLOAT, 0, vLine);

	glDrawElements(GL_LINES, 2, GL_UNSIGNED_BYTE, indices);

	glDisableClientState(GL_VERTEX_ARRAY);
}
void CDebugDraw::drawSphere(const btVector3& p, btScalar radius, const btVector3& color){

	glColor4f (color.getX(), color.getY(), color.getZ(), btScalar(1.0f));

	glPushMatrix ();

	glTranslatef (p.getX(), p.getY(), p.getZ());

	int lats = 5;

	int longs = 5;

	int i, j;

	GLfloat* v = (GLfloat*) kdMalloc(2 * (longs + 1) * 3 * sizeof(GLfloat));

	for(i = 0; i <= lats; i++) {
		btScalar lat0 = SIMD_PI * (-btScalar(0.5) + (btScalar) (i - 1) / lats);
		btScalar z0  = radius*sin(lat0);
		btScalar zr0 =  radius*cos(lat0);

		btScalar lat1 = SIMD_PI * (-btScalar(0.5) + (btScalar) i / lats);
		btScalar z1 = radius*sin(lat1);
		btScalar zr1 = radius*cos(lat1);

		glEnableClientState(GL_VERTEX_ARRAY);

		int k = 0;
		kdMemset(v, 0, sizeof(v));
		for(j = 0; j <= longs; j++) {
			btScalar lng = 2 * SIMD_PI * (btScalar) (j - 1) / longs;
			btScalar x = cos(lng);
			btScalar y = sin(lng);
			glNormal3f(x * zr0, y * zr0, z0);
			v[k]  = x * zr0;
			v[k+1]= y * zr0;
			v[k+2]= z0;
			glNormal3f(x * zr1, y * zr1, z1);
			v[k+3]  = x * zr1;
			v[k+4]= y * zr1;
			v[k+5]= z1;
		}
		glVertexPointer(3, GL_FLOAT, 0, v);
		glDrawArrays(GL_TRIANGLE_STRIP, 0, (longs + 1) * 2);
		glDisableClientState(GL_VERTEX_ARRAY);
	}
	glPopMatrix();
}


} /* namespace GGEngine */
